#pragma once
#include <iostream>
#include <list>
#include <iterator>
#include <set>
#include <boost/shared_ptr.hpp>
#include <boost/unordered_set.hpp>
#include <boost/unordered_map.hpp>
#include "SuccessorGenerator.h"
#include "Node.h"
#include "Map.h"
#include "Plan.h"

class SharedPtrNodeHash : public std::unary_function<boost::shared_ptr<const Node>, size_t>
{
public:
  SharedPtrNodeHash() { }
  size_t operator()(const boost::shared_ptr<const Node>& node) const
  {
    return _nodeHash(node.get());
  }
private:
  const NodeHash _nodeHash;
};

class SharedPtrNodeEqualTo : public std::binary_function<boost::shared_ptr<const Node>, boost::shared_ptr<const Node>, bool>
{
public:
  SharedPtrNodeEqualTo() { }
  bool operator()(const boost::shared_ptr<const Node>& node1, const boost::shared_ptr<const Node>& node2) const
  {
    return _nodeEqualTo(node1.get(), node2.get());
  }
private:
  const NodeEqualTo _nodeEqualTo;
};

class SharedPtrNodeCompare : public std::binary_function<boost::shared_ptr<const Node>, boost::shared_ptr<const Node>, bool>
{
public:
  SharedPtrNodeCompare() { }
  bool operator()(const boost::shared_ptr<const Node>& node1, const boost::shared_ptr<const Node>& node2) const
  {
    return _nodeCompare(node1.get(), node2.get());
  }
private:
  const NodeCompare _nodeCompare;
};

class Search
{
public:
	Search(boost::shared_ptr<const Map>& initialMap)
		: _initialMap(initialMap)
	{}

	boost::shared_ptr<const Plan> reconstructPlan(const boost::shared_ptr<Node>& node)
	{
		std::list<Action> actions;
		//boost::shared_ptr<Node> currentNode = node;
    boost::shared_ptr<const Node> currentNode = node;
		while (currentNode != nullptr && currentNode->getPredecessor() != nullptr)
		{
			actions.push_front(currentNode->getAction());
      currentNode = currentNode->getPredecessor();
		}

      boost::shared_ptr<const Node> newNode = currentNode->getPredecessor();
      currentNode = newNode;

		return boost::shared_ptr<const Plan>(new Plan(actions.begin(), actions.end(), node->getPoints()));
	}

protected:
	const boost::shared_ptr<const Map> _initialMap;
};

class DepthFirstSearch : public Search
{
public:
	DepthFirstSearch(boost::shared_ptr<const Map>& initialMap)
		: Search(initialMap)
	{}

	boost::shared_ptr<const Plan> search()
	{
		SuccessorGenerator successorGenerator;

    typedef boost::unordered_set<boost::shared_ptr<const Node>, SharedPtrNodeHash, SharedPtrNodeEqualTo> ExploredSet;
    //typedef std::set<boost::shared_ptr<const Node>, SharedPtrNodeCompare> ExploredSet;
    ExploredSet explored;
		std::list<boost::shared_ptr<Node>> open;

		boost::shared_ptr<Node> initialNode(new Node(_initialMap, 0));
		open.push_front(initialNode);

    boost::shared_ptr<Node> bestNode;
		while (!open.empty())
		{
			boost::shared_ptr<Node> node = open.front();
			open.pop_front();

      std::cout << *node -> getMap(); 

      std::pair<ExploredSet::iterator, bool> result = explored.insert(node);
      if (result.second) // If the node was successfully added to the set (i.e. not already explored)
      {
				if (bestNode == nullptr || *node > *bestNode)
				{
					bestNode = node;
				}

        //successorGenerator.generateSuccessors(node, std::front_inserter(open));
   
        std::list<boost::shared_ptr<Node>> successors;
        successorGenerator.generateSuccessors(node, std::back_inserter(successors));
        open.insert(open.begin(), successors.begin(), successors.end());
      }
		}
		return reconstructPlan(bestNode);
	}
};

class BreadthFirstSearch : public Search
{
public:
	BreadthFirstSearch(boost::shared_ptr<const Map>& initialMap)
		: Search(initialMap)
	{}

	boost::shared_ptr<const Plan> search()
	{
		SuccessorGenerator successorGenerator;

    typedef boost::unordered_set<boost::shared_ptr<const Node>, SharedPtrNodeHash, SharedPtrNodeEqualTo> ExploredSet;
    ExploredSet explored;
		std::list<boost::shared_ptr<Node>> open;

		boost::shared_ptr<Node> initialNode = boost::shared_ptr<Node>(new Node(_initialMap, 0));
		open.push_front(initialNode);

    boost::shared_ptr<Node> bestNode;
		while (!open.empty())
		{
			boost::shared_ptr<Node> node = open.front();
			open.pop_front();
      std::pair<ExploredSet::iterator, bool> result = explored.insert(node);
      if (result.second) // If the node was successfully added to the set (i.e. not already explored)
      {
				if (bestNode == nullptr || *node > *bestNode)
				{
					bestNode = node;
				}
        successorGenerator.generateSuccessors(node, std::back_inserter(open));
      }
		}
		return reconstructPlan(bestNode);
	}
};